#ifndef _VEHICLE_MONITOR_H_
#define _VEHICLE_MONITOR_H_

#include <dispatch_server/global.h>
#include <dispatch_system/vehicle.h>
#include <moying_navigation_msgs/LocalCostmap.h>
#include <dispatch_system_ros_viz/vehicle_viz.h>
#include <moying_proto/moying_proto.h>
#include <moying_comm_lib/waiter.h>
#include <moying_comm_lib/publish.h>
#include <chrono>
#include <dispatch_server/revise_vehicle_path.h>

//最大重规划的次数
#define MAX_REPLAN_TIMES 10

namespace moying
{
namespace navigation
{
    struct GetPathPointResult
    {
        bool error;
        
    };
    class VehicleMonitor
    {
        public:
            VehicleMonitor(const MoyingMapPtr &moyingmap, const VehiclePtr &vehicle, const visualization::VehicleVizPtr &viz, const communication::MoyingCommuPublishPtr &path_pub, const std::string &path_pub_topic);
            virtual ~VehicleMonitor();
            bool initialize();

            //检测机器人的导航状态,决定何时需要重规划路径
            void check();
            
            // void localCostmapCallback(const moying_proto::LocalCostMap &local_costmap);
            void localObstacleCallback(const moying_proto::LocalObstacle &local_obstacle);

            void notifyArrived(const moying_proto::NavStateNotify &notify);
            void startMonitoring(navigation::PlanningResult &result);
            void publishAStarCloseNodes(const boost::dynamic_bitset<>& closed_);
            bool clearTimemapFootprint();
            bool tryEmergencyRecovery();
            bool isTracking(){return tracking_;}
            bool cancelNav();
            void stopRobot(int stop_path_index);
            bool checkOneWayRoadCanPass(const navigation::PlanningResult &result, int track_path_index=0);//检测接下来的路径点是否可以通过单向道

        private:
            // bool isLocalObstacleFree(int x, int y,float theta=0);
            
            void notifyPath();
            //重规划路径
            bool pathReplan(int path_plan_begin_time=0,bool is_revise = false);
            void publishVisualization();
            
            //预检测导航路径点是否有障碍物
            bool precheckPathPointValid(int* stop_path_index=nullptr,int* collide_path_index=nullptr);
            //更新剩余的路径点到达时刻
            bool updateRestPathPointArriveTime(int rest_start_index);
            void stopMonitoring();
            bool isPointValid(unsigned int point_index);
            void inflateObstacle(double inflation_radius,LocalObstacleInfo& obs_info);

        private:
            ros::NodeHandle nh_;
            visualization::VehicleVizPtr viz_;
            moying::communication::MoyingCommuPublishPtr path_publisher_;
            std::string path_pub_topic_;

            VehiclePtr vehicle_;
            MoyingMapPtr moyingmap_;
            CostMapPtr costmap_;
            
            navigation::PlanningResult result_;

            unsigned int track_path_index_=0;
            unsigned int collide_path_index_=0;
            int max_replan_times_   = MAX_REPLAN_TIMES;    //最多尝试路径重规划的次数


            bool tracking_ = false;
            bool checked_nav_stop_ = false;     //是否检测到导航停止
            bool emergent_obstacle_detect_ = false;  // 检测到紧急障碍物
            std::chrono::system_clock::time_point check_stop_time_; //机器人急停\暂停或者检测到会发生障碍物碰撞的时间
            int precheck_path_point_count_ = 2; //需要进行局部障碍物检测的路径点数量
            bool path_replaning_ = false;    //标识是否正在进行路径重规划
            std::chrono::system_clock::time_point last_try_recovery_time_;  //上一次尝试紧急障碍物恢复的时间 

            ReviseVehiclePath revise_vehicle_path_;//修正机器人路径
            int path_follow_timeout_=0; //路径跟随的超时秒数
            float precheck_one_way_road_can_pass_len_;  //预检测单向道是否可以通行的长度

    };
    typedef std::shared_ptr<VehicleMonitor> VehicleMonitorPtr;
}
}
#endif //_VEHICLE_MONITOR_H_